/**
 * @file pid.c
 * @brief PID controller implementation (positional and incremental forms)
 * @author 废话文学创始人
 * @version v1.0
 * @date 2024-07-14
 * @note Supports positional PID and incremental PID with various enhancements
 */
#include <pid.h>
#include <stdlib.h>
#include "math.h"

/**
 * @brief Set PID parameters (Kp, Ki, Kd)
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] kp Proportional gain
 * @param[in] ki Integral gain
 * @param[in] kd Derivative gain
 */
void PID_Parameter_Set(PID *pid, float kp, float ki, float kd)
{
    pid->parameter.kp = kp;
    pid->parameter.ki = ki;
    pid->parameter.kd = kd;
}
/**
 * @brief Set output offset for static friction compensation
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] offset Output offset value
 * @note Larger offset values require larger error tolerance range
 */
static void PID_Output_Offset_Set(PID *pid, float offset)
{
    pid->output.offset = offset;
}
/**
 * @brief Set upper limit for output saturation
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] output_upper_limit Maximum output value
 */
static void PID_OutPut_Upper_Limit_Set(PID *pid, float output_upper_limit)
{
    pid->output.upper_limit = output_upper_limit;
}
/**
 * @brief Set complementary filter parameter for derivative term
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] parameter_filter_for_D Filter parameter (0.0 to 1.0)
 */
static void PID_Differential_Parameter_Set(PID *pid, float parameter_filter_for_D)
{
    pid->parameter.filter_for_D = parameter_filter_for_D;
}
/**
 * @brief Set error tolerance range for error filtering
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] error_tolerance_range Error threshold for filtering
 */
static void PID_Error_Tolerance_Range_Set(PID *pid, float error_tolerance_range)
{
    pid->parameter.error_tolerance_range = error_tolerance_range;
}
/**
 * @brief Initialize PID controller with all parameters
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] kp Proportional gain
 * @param[in] ki Integral gain
 * @param[in] kd Derivative gain
 * @param[in] output_offset Output offset for friction compensation
 * @param[in] output_upper_limit Maximum output limit
 * @param[in] parameter_filter_for_D Derivative filter parameter
 * @param[in] error_tolerance_range Error filtering threshold
 * @param[in] pidControl Callback function for actual control output
 * @param[in] pidGearshiftIntegral Callback function for variable integral gain (NULL if not used)
 */
void PID_Init(PID *pid, float kp, float ki, float kd, float output_offset, float output_upper_limit, float parameter_filter_for_D, float error_tolerance_range, Pid_Control pidControl, Pid_Gearshift_Integral pidGearshiftIntegral)
{
    pid->parameter.kp = kp;
    pid->parameter.ki = ki;
    pid->parameter.kd = kd;
    PID_Output_Offset_Set(pid, output_offset);
    PID_OutPut_Upper_Limit_Set(pid, output_upper_limit);
    PID_Differential_Parameter_Set(pid, parameter_filter_for_D);
    PID_Error_Tolerance_Range_Set(pid, error_tolerance_range);
    if (pidControl != NULL)
        pid->pidControl = pidControl;
    if (pidGearshiftIntegral != NULL)
        pid->pidGearshiftIntegral = pidGearshiftIntegral;
}
/**
 * @brief Set target value for PID controller
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] target Desired setpoint value
 */
void PID_Target_Set(PID *pid, float target)
{
    pid->input.target = target;
}
/**
 * @brief Get current target value from PID controller
 * @param[in] pid PID controller instance
 * @return Current target value
 */
float PID_Target_Get(PID pid)
{
    return pid.input.target;
}
/**
 * @brief Get current output value from PID controller
 * @param[in] pid PID controller instance
 * @return Current output value
 */
float PID_Output_Get(PID pid)
{
    return pid.output.value;
}
/**
 * @brief Apply output saturation limits
 * @param[in,out] pid Pointer to PID controller instance
 */
static void PID_OutPut_Limit(PID *pid)
{
    if (pid->output.value >= pid->output.upper_limit)
        pid->output.value = pid->output.upper_limit;
    else if (pid->output.value <= -pid->output.upper_limit)
        pid->output.value = -pid->output.upper_limit;
}
/**
 * @brief Apply derivative independent filtering for positional PID
 * @param[in,out] pid Pointer to PID controller instance
 * @note Uses complementary filter for derivative term
 */
static void PID_Differential_Independent_Position(PID *pid)
{
    pid->output.D = -((1 - pid->parameter.filter_for_D) * pid->parameter.kd * (pid->input.actual - pid->input.actual_previous) + pid->parameter.filter_for_D * pid->output.D);
}
/**
 * @brief Filter small errors below tolerance threshold
 * @param[in,out] pid Pointer to PID controller instance
 * @note Errors within tolerance range are set to zero
 */
static void PID_Error_Filter(PID *pid)
{
    if (abs((int)(pid->error.current)) <= (int)pid->parameter.error_tolerance_range)
    {
        pid->error.current = 0;
    }
}
/**
 * @brief Apply variable integral gain (anti-windup)
 * @param[in,out] pid Pointer to PID controller instance
 * @note Prevents integral windup and excessive integral accumulation
 */

static void PID_KI_Gearshift_Integral(PID *pid)
{
    if (fabs((double)(pid->parameter.ki)) > 0.01)
    {
        pid->parameter.Gearshift_Integral = pid->pidGearshiftIntegral(pid->error.current);
        pid->error.integral += pid->parameter.Gearshift_Integral * pid->error.current;
    }
}
/**
 * @brief Apply output offset for static friction compensation
 * @param[in,out] pid Pointer to PID controller instance
 * @note Adds offset to overcome static friction, sets to zero for very small outputs
 */
static void PID_Output_Offset(PID *pid)
{
    if (pid->output.value >= pid->output.offset / 10)
    {
        pid->output.value = pid->output.value + pid->output.offset;
    }
    else if (pid->output.value <= pid->output.offset / 10)
    {
        pid->output.value = pid->output.value - pid->output.offset;
    }
    else
    {
        pid->output.value = 0;
    }
}
/**
 * @brief Execute positional PID regulation
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] actual Current measured value
 * @note Should be called at regular intervals (millisecond level)
 */
void PID_Regulation_Position(PID *pid, float actual)
{
    // 获取当前实际值
    pid->input.actual_previous = pid->input.actual;
    pid->input.actual = actual;
    // 误差计算
    pid->error.previous = pid->error.current;
    pid->error.current = pid->input.target - pid->input.actual;
    // 误差积分
    // 误差过滤，过滤更极小的那部分误差
    PID_Error_Filter(pid);
    // 变速积分
    PID_KI_Gearshift_Integral(pid);
    // 微分先行以及不完全微分
    PID_Differential_Independent_Position(pid);
    // pid计算
    pid->output.value =
        pid->parameter.kp * pid->error.current +
        pid->parameter.ki * pid->error.integral +
        pid->output.D;
    // 输出偏移
    PID_Output_Offset(pid);
    // 输出限幅
    PID_OutPut_Limit(pid);
    // 进行实际的PID控制
    pid->pidControl(pid->output.value);
}
/**
 * @brief Apply derivative independent filtering for incremental PID
 * @param[in,out] pid Pointer to PID controller instance
 */
static void PID_Differential_Independent_Incremental(PID *pid)
{
    pid->output.D = -((1 - pid->parameter.filter_for_D) * pid->parameter.kd * (pid->input.actual - 2 * pid->input.actual_previous + pid->input.actual_previous2) + pid->parameter.filter_for_D * pid->output.D);
}
/**
 * @brief Execute incremental PID regulation
 * @param[in,out] pid Pointer to PID controller instance
 * @param[in] actual Current measured value
 * @note Should be called at regular intervals (millisecond level)
 */
void PID_Regulation_Incremental(PID *pid, float actual)
{
    // 获取当前实际值
    pid->input.actual_previous2 = pid->input.actual_previous;
    pid->input.actual_previous = pid->input.actual;
    pid->input.actual = actual;
    // 误差计算
    pid->error.previous2 = pid->error.previous;
    pid->error.previous = pid->error.current;
    pid->error.current = pid->input.target - pid->input.actual;
    // 误差过滤，过滤更极小的那部分误差
    PID_Error_Filter(pid);
    // 微分先行以及不完全微分
    PID_Differential_Independent_Incremental(pid);
    // pid计算
    pid->output.variation =
        pid->parameter.kp * (pid->error.current - pid->error.previous) +
        pid->parameter.ki * pid->error.current +
        pid->output.D;
    pid->output.value = pid->output.value + pid->output.variation;
    // 输出偏移
    PID_Output_Offset(pid);
    // 输出限幅
    PID_OutPut_Limit(pid);
    // 进行实际的PID控制
    pid->pidControl(pid->output.value);
}
